package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Solenoid;
import java.util.TimerTask;

/**
 *
 * @author buttercream
 */
public class MinibotDeploy extends TimerTask
{
    private Solenoid botDeploy;
    private Solenoid drawerDeploy;
    private Joystick botJoy;
   // private DigitalInput poleSwitch;
    private AnalogLimitSwitch poleSwitch;
    private boolean isDeployed = false;
    private IGingersnapCB cb;
    
    MinibotDeploy(Joystick botJoy, IGingersnapCB cb) {
        this.botJoy = botJoy;
        this.cb = cb;

        AnalogChannel channel = new AnalogChannel(Constants.CRIO_ANALOG_BREAKOUT, Constants.MINIBOT_LIMIT_SWITCH);
        poleSwitch = new AnalogLimitSwitch( channel);

       // poleSwitch = new DigitalInput( Constants.CRIO_ANALOG_BREAKOUT, Constants.MINIBOT_LIMIT_SWITCH );
        botDeploy = new Solenoid(Constants.CRIO_PNEUMATICS, Constants.SOL_BOT_DEPLOY);
        drawerDeploy = new Solenoid(Constants.CRIO_PNEUMATICS, Constants.SOL_DRAWER_DEPLOY);
    }

    public void run() {
        if(botJoy.getRawButton(Constants.BTN_MINIBOT_DEPLOY_ONE) &&
           botJoy.getRawButton(Constants.BTN_MINIBOT_DEPLOY_TWO)) {
            drawerDeploy.set(true);
            isDeployed = true;
        }

        if(isDeployed && poleSwitch.readSwitch()) {
            cb.stopDrive();
            botDeploy.set(true);
        }

        if(isDeployed && botJoy.getRawButton(Constants.BTN_MINITBOT_RETRACT)) {
            drawerDeploy.set(false);
            cb.resumeDrive();
            isDeployed = false;
        }
    }
}
